Debugging Forward Kinematics

Once you have individual transforms across joints and the complete homogeneous transform, you can validate them by comparing your results to model outputs. From a terminal window, launch the forward kinematics demo:

$ roslaunch kuka_arm forward_kinematics.launch

Executing the roslaunch command should spawn the robot arm in RViz along with a “joint state publisher window”. If successful, you should see:

Notice checkboxes for the following displays on left side panel: (1) RobotModel and (2) TF. Play with them, expand their menus and experiment turning them on and off.

To keep things simple, base_link of the robot is kept at the origin of world coordinate frame.

While you can easily obtain the transform between parent and child links from the TF display on the left side panel, ROS makes it very easy to get the transform between any two given frames with the tf_echo command. The generic syntax for this command is:

$ rosrun tf tf_echo [reference frame] [target frame]

Again, the frames used in the KUKA arm are found as a drop down menu under the TF topic.

Open another terminal window and type:

$ rosrun tf tf_echo base_link link_6

Position and orientation data of link_6 relative to the base_link should begin streaming in your terminal window. You can press Ctrl+C to stop the stream.

The translation (distance between link origins) has units of meters while the rotation is given as quaternions and Roll-Pitch-Yaw angles.

You can use the tf_echo command to check the transforms between the robot links and your calculations to verify that each individual transform is correct.

Alternatively, you can use the joint_state_publisher window as well. After you calculate your final transformation matrix between the base_link and gripper_link, you can substitute the joint angles with values of your own to obtain the orientation and position of the end-effector. You can substitute those joint angle values by using the sliders in the joint_state_publisher window. The resulting position and orientation (quaternions) of your gripper_link is then shown under its frame in the TF topic which can help confirm your calculations:

Finally, spend some time playing with the joint_state_publisher. As you increase the joint angle, pay attention to which joint is changing and the direction of rotation associated with positive and negative motions. This will help you define the rotation about any given joint in the DH figure you construct.

When you have finished exploring the forward kinematics demo, you can kill all the active processes by pressing Ctrl+C in the terminal window where you launched it.